/* 
 * $Id$
 *
 * UMKC Robotics 2009
**/

#include <math.h>
#include "path.h"
#include "2space.h"

#include "err.h"
extern int err;

#ifdef DBG
#include "iopublic.h"
extern const BotIoctl bioctl;
#endif




/* static forwards */
static void p_reset(Path*);
static inline int p_make(Path*);
static inline void p_set_start_polar(Path*, PolarPoint*);
static inline void p_set_start_rect(Path*, RectangularPoint*);
static inline void p_set_end_polar(Path*, PolarPoint*);
static inline void p_set_end_rect(Path*, RectangularPoint*);
static inline void p_set_num_verticies(Path*, int);
static inline void p_set_make_F(Path*, Fpath);
static PolarPoint p_get_pos_polar(Path*, int);
static RectangularPoint p_get_pos_rect(Path*, int);

/* default path builder - which is not in this file. */
extern int p_linear_segmentation(Path*);

/* Path controls */
const PathCtl pathctl = {
  &p_reset,
  &p_make,
  &p_set_start_polar,
  &p_set_start_rect,
  &p_set_end_polar,
  &p_set_end_rect,
  &p_set_num_verticies,
  &p_set_make_F,
  &p_get_pos_polar,
  &p_get_pos_rect
};



/*-------------------------------------------------------*/
/* implementation below here */
/*-------------------------------------------------------*/

/* reset a path structure to all defaults */
static void p_reset(Path* p) {

  int i;
  p->a = 0.0;
  p->numverts = 0;
  p->path_builder_f = &p_linear_segmentation;
  for(i=0; i<MAX_VERTICIES; i++ )
    p->r[i] = p->x[i] = p->y[i] = (Float_T) 0.0;

}

static inline int p_make(Path* p) {

  if(p->path_builder_f != NULL ) 
    return( (*p->path_builder_f)(p) );
  else
    return( err = E_NULL_PATH_FUNC );

}

static inline void p_set_start_polar(Path* p, PolarPoint* pp) {

  p->r[START_VERTEX] = pp->r;
  p->a = pp->a;

}


static inline void p_set_start_rect(Path* p, RectangularPoint* rp) {

  p->x[START_VERTEX] = rp->x;
  p->y[START_VERTEX] = rp->y;

}

static inline void p_set_end_polar(Path* p, PolarPoint* pp) {

  if( p->numverts > 0 ) 
    p->r[END_VERTEX(p)] = pp->r;
  else 
    err = E_INVALID_ARG; 

}

static inline void p_set_end_rect(Path* p, RectangularPoint* rp) {

  if( p->numverts > 0 ) {
    p->x[END_VERTEX(p)] = rp->x;
    p->y[END_VERTEX(p)] = rp->y;
  } else 
    err = E_INVALID_ARG; 

}

static inline void p_set_num_verticies(Path* p, int v) {

  if(v > 1 && v <= MAX_VERTICIES)
    p->numverts = v;
  else
    err = E_INVALID_ARG;

}

static inline void p_set_make_F(Path* p, Fpath Fp) {

  p->path_builder_f = Fp;

}

static PolarPoint p_get_pos_polar(Path* p, int idx) {

  PolarPoint pp;  
  if( idx >= 0 && idx <= END_VERTEX(p) ) {
    pp.r = p->r[idx];
    pp.a = p->a;
  } else
    err = E_IDX_OOB;
  return( pp );

}

static RectangularPoint p_get_pos_rect(Path* p, int idx) {

  RectangularPoint rp;
  if( idx >= 0 && idx <= END_VERTEX(p) ) {
    rp.x = p->x[idx];
    rp.y = p->y[idx];
  } else
    err = E_IDX_OOB;
  return( rp );

}
